using BepuUtilities;
using BepuPhysics;
using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection;
using BepuPhysics.Constraints;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
using System;
#endif

namespace Demos
{
    public struct DemoPoseIntegratorCallbacks : IPoseIntegratorCallbacks
    {
        public Vector3 Gravity;
        public float LinearDamping;
        public float AngularDamping;
        Vector3 gravityDt;
        float linearDampingDt;
        float angularDampingDt;

        public AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;

        public DemoPoseIntegratorCallbacks(Vector3 gravity, float linearDamping = .03f, float angularDamping = .03f) : this()
        {
            Gravity = gravity;
            LinearDamping = linearDamping;
            AngularDamping = angularDamping;
        }

        public void PrepareForIntegration(float dt)
        {
            // 没有理由为每个人重新计算重力*dt;只需提前缓存即可。
            gravityDt = Gravity * dt;
            // 因为这不使用每个物体的阻尼,所以我们可以预先计算所有的东西。
            linearDampingDt = MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt);
            angularDampingDt = MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia localInertia, int workerIndex, ref BodyVelocity velocity)
        {
            // 请注意,我们避免加速运动学。运动学是指任何质量为零的物体(即质量为~无穷大的物体)。没有任何力量可以移动它们。
            if (localInertia.InverseMass > 0)
            {
                velocity.Linear = (velocity.Linear + gravityDt) * linearDampingDt;
                velocity.Angular = velocity.Angular * angularDampingDt;
            }
            // 实现侧记：为什么不将所有运动学与动力学分开捆绑在一起,以避免这种逐个身体的情况？
            // 因为运动学可以有速度-这是它们与静电对象的不同之处。求解器必须读取约束中涉及的所有实体的速度。
            // 在理想情况下,这些主体将位于内存附近,以增加缓存命中的机会。如果运动学单独捆绑,则缓存的数量
            // 未命中率必然会增加。为了加快姿势积分器的速度而减慢解算器的速度是一个非常非常糟糕的交易,特别是当好处是几个ALU操作时。

            // 请注意,从技术上讲,您可以通过Simulation.Bodies.ActiveSet.Poses直接访问IntegrateVelocity中的姿势,它只需要一点细心,不会直接公开。
            // 如果正在使用PositionFirstTimestpper,则姿势积分器已经集成了姿势。
            // 如果PositionLastTimestpper或SubsteppingTimestpper正在使用,则姿势尚未集成。
            // 如果姿势修改取决于集成顺序,则需要考虑这一点。

            // 这也是实现位置相关重力或身体阻尼等功能的方便之处。
        }

    }
    public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks
    {
        public SpringSettings ContactSpringiness;

        public void Initialize(Simulation simulation)
        {
            // 如果弹性度值未初始化,请使用默认值。
            if (ContactSpringiness.AngularFrequency == 0 && ContactSpringiness.TwiceDampingRatio == 0)
                ContactSpringiness = new SpringSettings(30, 1);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b)
        {
            // 虽然引擎根本不会尝试在静力学之间创建对,但它会询问运动学-运动学对。
            // 这些对不能发射约束,因为涉及的两个物体都有无限大的惯性。因为大多数演示不需要
            // 要收集有关运动学-运动学对的信息,我们将要求至少有一个实体需要是动力学的。
            return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
        {
            return true;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : struct, IContactManifold<TManifold>
        {
            pairMaterial.FrictionCoefficient = 1f;
            pairMaterial.MaximumRecoveryVelocity = 2f;
            pairMaterial.SpringSettings = ContactSpringiness;
            return true;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
        {
            return true;
        }

        public void Dispose()
        {
        }
    }

}
